/*
 * Copyright (c) 2006-2021, RT-Thread Development Team
 *
 * SPDX-License-Identifier: Apache-2.0
 *
 * Change Logs:
 * Date           Author       Notes
 * 2023-07-10     YogaX1       the first version
 */
#include <rtthread.h>
#include <rtdevice.h>
#include "drv_gpio.h"
#include "stepper_motor.h"

int16_t direction;              // Direction of rotation
uint32_t step_delay;            // delay between steps, in ms, based on speed
uint32_t number_of_steps;       // total number of steps this motor can take for one revolution
float duty_ratio;               // duty ratio of STP signal, by default it's 0.5, and it seems that it will influence the speed
int32_t accumulated_steps;      // a counter tracking the number of steps that the motor has moved

// motor pin numbers:
uint32_t motor_pin_dis;
uint32_t motor_pin_stp;
uint32_t motor_pin_dir;

uint32_t last_step_time;        // time stamp in us of when the last step was taken

/*
 * two-wire constructor.
 * Sets which wires should control the motor.
 */
void stepper_motor_control(uint16_t steps, uint16_t pin_dir, uint16_t pin_stp, uint16_t pin_dis)
{
    direction = 0;      // motor direction
    last_step_time = 0; // time stamp in us of the last step taken
    number_of_steps = steps; // total number of steps for this motor
    duty_ratio = 0.5;
    accumulated_steps = 0;

    // set the speed at 1 rpm, same as default speed
    step_delay = 60L * 1000L * 1000L / (number_of_steps * 1);

    // pins for the motor control connection:
    motor_pin_dir = pin_dir;
    motor_pin_stp = pin_stp;
    motor_pin_dis = pin_dis;
}

void stepper_motor_pininit(void)
{
    // Set all Pins to LOW
    rt_pin_write(motor_pin_dir, PIN_LOW);
    rt_pin_write(motor_pin_stp, PIN_LOW);
    rt_pin_write(motor_pin_dis, PIN_LOW);

    // setup the pins on the micro-controller:
    rt_pin_mode(motor_pin_dir, PIN_MODE_OUTPUT);
    rt_pin_mode(motor_pin_stp, PIN_MODE_OUTPUT);
    rt_pin_mode(motor_pin_dis, PIN_MODE_OUTPUT);
}

void stepper_motor_enable()
{
    rt_pin_write(motor_pin_dis, PIN_HIGH);
}

void stepper_motor_disable()
{
    rt_pin_write(motor_pin_dis, PIN_LOW);
}

void stepper_motor_begin()
{
    stepper_motor_pininit();

    // set the default speed at 1 rpm
    step_delay = 60L * 1000L * 1000L / (number_of_steps * 1);

    stepper_motor_enable();
}

void stepper_motor_end()
{
    stepper_motor_disable();

    // set the speed at 1 rpm, same as default speed
    step_delay = 60L * 1000L * 1000L / (number_of_steps * 1);

    // setup the pins on the micro-controller:
    rt_pin_mode(motor_pin_dir, PIN_MODE_INPUT);
    rt_pin_mode(motor_pin_stp, PIN_MODE_INPUT);
    rt_pin_mode(motor_pin_dis, PIN_MODE_INPUT);
}

void stepper_motor(bool high_low)
{
    rt_pin_write(motor_pin_stp, high_low);
}

void stepper_motor_update_accumulated_steps(int16_t steps)
{
    accumulated_steps += steps;

    if (accumulated_steps >= number_of_steps)
    {
        accumulated_steps -= number_of_steps;
    }

    if (accumulated_steps <= -(number_of_steps))
    {
        accumulated_steps += number_of_steps;
    }
}

/*
 * Sets the speed in revolutions per minute
 */
void stepper_motor_set_speed(float whatSpeed)
{
    step_delay = 60L * 1000L * 1000L / (number_of_steps * whatSpeed);
}

void stepper_motor_set_duty_ratio(float whatRatio)
{
    duty_ratio = whatRatio;
}

static rt_uint32_t get_tick_val(void)
{
    return SysTick->VAL;
}

static rt_uint32_t get_tick_load(void)
{
    return SysTick->LOAD;
}

static rt_uint32_t tick_start, tick_reload;

void tick_begin(void)
{
    tick_start = get_tick_val();
    tick_reload = get_tick_load();
}

rt_uint32_t tick_end(void)
{
    rt_uint32_t tick_ms = SystemCoreClock / 1000UL;
    rt_uint32_t now;

    now = get_tick_val();
    return ((tick_start > now ? tick_start - now : tick_reload + tick_start - now) / tick_ms);
}

/*
 * Moves the motor steps_to_move steps.  If the number is negative,
 * the motor moves in the reverse direction.
 */
void stepper_motor_move_step(int16_t steps_to_move)
{
    stepper_motor_update_accumulated_steps(steps_to_move);

    uint16_t steps_left = abs(steps_to_move);  // how many steps to take

    // determine direction based on whether steps_to_mode is + or -:
    if (steps_to_move > 0)
    {
        direction = 1;
    }
    if (steps_to_move < 0)
    {
        direction = 0;
    }

    rt_pin_write(motor_pin_dir, direction);

    // decrement the number of steps, moving one step each time:
    while (steps_left > 0)
    {
        tick_begin();
        uint32_t now = tick_start;

        while (now - last_step_time < (duty_ratio * step_delay))
        {
            now = tick_end();
        }

        // get the timeStamp of when the motor half stepped:
        last_step_time = now;

        // output a HIGH half step to STP pin
        stepper_motor(PIN_HIGH);

        while (now - last_step_time < (duty_ratio * step_delay))
        {
            now = tick_end();
        }

        // get the timeStamp of when the motor half stepped:
        last_step_time = now;

        // output a LOW half step to STP pin
        stepper_motor(PIN_LOW);

        // decrement the steps left only after a LOW half step:
        steps_left--;
    }
}

void stepper_motor_move_degree(float number_of_degree)
{
    if (number_of_degree > 0)
    {
        stepper_motor_move_step((int32_t) (number_of_steps * number_of_degree / 360));
    }
    else
    {
        stepper_motor_move_step((int32_t) (number_of_steps * number_of_degree / 360));
    }
}

void stepper_motor_move_revolution(float number_of_revolution)
{
    stepper_motor_move_step((int32_t) (number_of_steps * number_of_revolution));
}

/*
 * Moves the motor forward or backwards.
 */
void stepper_motor_reset_motor()
{
    stepper_motor_move_step(-accumulated_steps);
}

/*
 version() returns the version of the library:
 */
uint16_t stepper_motor_version(void)
{
    return 1;
}
